#include "port_i2c.h"
#include <stdio.h>
#include "driver/i2c.h"
#include "sdkconfig.h"
#include "port_config.h"

#define I2C_CH_NUM  (SOC_I2C_NUM)
#define IS_I2C_CH(ch) ((ch) < I2C_CH_NUM)

static i2c_config_t i2c_conf[I2C_CH_NUM] = {
    {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = GPIO_I2C_CH0_SDA,         // select GPIO specific to your project
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_io_num = GPIO_I2C_CH0_SCL,         // select GPIO specific to your project
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = 400000,  // select frequency specific to your project
        // .clk_flags = 0,          /*!< Optional, you can use I2C_SCLK_SRC_FLAG_* flags to choose i2c source clock here. */
    },
    {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = GPIO_I2C_CH1_SDA,         // select GPIO specific to your project
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_io_num = GPIO_I2C_CH1_SCL,         // select GPIO specific to your project
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = 400000,  // select frequency specific to your project
        // .clk_flags = 0,          /*!< Optional, you can use I2C_SCLK_SRC_FLAG_* flags to choose i2c source clock here. */
    },
};

uint8_t i2c_init(uint8_t ch)
{
    esp_err_t err = 0;
    if(IS_I2C_CH(ch))
    {
        err =  i2c_param_config(ch, &i2c_conf[ch]);
        if(err != ESP_OK)
        {
            printf("ERROR i2c param set error:0x%X\n", err);
            return 1;
        }
        err =  i2c_driver_install(ch, I2C_MODE_MASTER, 0, 0, 0);
        if(err != ESP_OK)
        {
            printf("ERROR i2c param set error:0x%X\n", err);
            return 1;
        }
        return 0;
    }
    return 1;
}

uint8_t i2c_check(uint8_t ch, uint8_t slave_addr)
{
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (slave_addr << 1) | I2C_MASTER_WRITE, true);
    i2c_master_stop(cmd);
    esp_err_t err = i2c_master_cmd_begin(ch, cmd, 1000 / portTICK_RATE_MS);
    i2c_cmd_link_delete(cmd);
    if(err != ESP_OK)
    {
        return 1;
    }
    return 0;
}

uint8_t i2c_write_bytes(uint8_t ch, uint8_t slave_addr, uint8_t reg_addr, uint8_t* data, uint8_t len)
{
    if(IS_I2C_CH(ch))
    {
        if(len == 0)
        {
            return 0;
        }
        
        i2c_cmd_handle_t cmd = i2c_cmd_link_create();
        i2c_master_start(cmd);
        i2c_master_write_byte(cmd, (slave_addr << 1) | I2C_MASTER_WRITE, true);
        i2c_master_write_byte(cmd, reg_addr, true);
        i2c_master_write(cmd, data, len, true);
        i2c_master_stop(cmd);
        esp_err_t err = i2c_master_cmd_begin(ch, cmd, 1000 / portTICK_RATE_MS);
        i2c_cmd_link_delete(cmd);
        if(err != ESP_OK)
        {
            printf("ERROR i2c write error:0x%X\n", err);
            return 1;
        }
        return 0;
    }
    return 1;
}

uint8_t i2c_write_mem(uint8_t ch, uint8_t slave_addr, uint8_t* data, uint8_t len)
{
    if(IS_I2C_CH(ch))
    {
        if(len == 0)
        {
            return 0;
        }
        
        i2c_cmd_handle_t cmd = i2c_cmd_link_create();
        i2c_master_start(cmd);
        i2c_master_write_byte(cmd, (slave_addr << 1) | I2C_MASTER_WRITE, true);
        i2c_master_write(cmd, data, len, true);
        i2c_master_stop(cmd);
        esp_err_t err = i2c_master_cmd_begin(ch, cmd, 1000 / portTICK_RATE_MS);
        i2c_cmd_link_delete(cmd);
        if(err != ESP_OK)
        {
            printf("ERROR i2c write error:0x%X\n", err);
            return 1;
        }
        return 0;
    }
    return 1;
}

uint8_t i2c_read_bytes(uint8_t ch, uint8_t slave_addr, uint8_t reg_addr, uint8_t* data, uint8_t len)
{
    if(IS_I2C_CH(ch))
    {
        if (len == 0) 
        {
            return 0;
        }
        i2c_cmd_handle_t cmd = i2c_cmd_link_create();
        i2c_master_start(cmd);
        i2c_master_write_byte(cmd, (slave_addr << 1) | I2C_MASTER_WRITE, true);
        i2c_master_write_byte(cmd, reg_addr, true);
        i2c_master_start(cmd);
        i2c_master_write_byte(cmd, (slave_addr << 1) | I2C_MASTER_READ, true);
        if (len > 1) 
        {
            i2c_master_read(cmd, data, len - 1, I2C_MASTER_ACK);
        }
        i2c_master_read_byte(cmd, data + len - 1, I2C_MASTER_LAST_NACK);
        i2c_master_stop(cmd);
        esp_err_t err = i2c_master_cmd_begin(ch, cmd, 1000 / portTICK_RATE_MS);
        i2c_cmd_link_delete(cmd);
        if(err != ESP_OK)
        {
            printf("ERROR i2c read error:0x%X\n", err);
            return 1;
        }
        return 0;
    }
    return 1;
}

uint8_t i2c_read_mem(uint8_t ch, uint8_t slave_addr, uint8_t* data, uint8_t len)
{
    if(IS_I2C_CH(ch))
    {
        if (len == 0) 
        {
            return 0;
        }
        i2c_cmd_handle_t cmd = i2c_cmd_link_create();
        i2c_master_start(cmd);
        i2c_master_write_byte(cmd, (slave_addr << 1) | I2C_MASTER_READ, true);
        if (len > 1) 
        {
            i2c_master_read(cmd, data, len - 1, I2C_MASTER_ACK);
        }
        i2c_master_read_byte(cmd, data + len - 1, I2C_MASTER_LAST_NACK);
        i2c_master_stop(cmd);
        esp_err_t err = i2c_master_cmd_begin(ch, cmd, 1000 / portTICK_RATE_MS);
        i2c_cmd_link_delete(cmd);
        if(err != ESP_OK)
        {
            printf("ERROR i2c read error:0x%X\n", err);
            return 1;
        }
        return 0;
    }
    return 1;
}